using System;

namespace BOL.Maths.LinearAlgebra
{
	/// <summary>
    /// QR Decomposition.
	/// For an m-by-n matrix A with m >= n, the QR decomposition is an m-by-n
	/// orthogonal matrix Q and an n-by-n upper triangular matrix R so that
	/// A = Q * R.
	/// 
	/// The QR decompostion always exists, even if the matrix does not have
	/// full rank, so the constructor will never fail.  The primary use of the
	/// QR decomposition is in the least squares solution of nonsquare systems
	/// of simultaneous linear equations.
	/// </summary>
	public sealed class QRDecomposition : DecompositionBase
	{
		#region Private members
		
        private double[][] _qr; // matrix for decomposition.
        private double[] _diagR; // diagonal vector of R, diag(R)

		#endregion

		#region Public Properties

        /// <summary>Returns true if R, and hence A has full rank.</summary>
		public bool IsFullRank
		{
			get
			{
				for (int j = 0; j < _n; j++)
					if (_diagR[j] == 0)
						return false;

				return true;
			}
		}

        /// <summary>Returns the householder matrix. Lower trapezoidal matrix whose columns define the reflections.</summary>
		public double[][] H
		{
			get
			{
                double[][] h = new double[_m][];
                int i, j;
                for (i = 0; i < _m; i++)
                {
                    h[i] = new double[_n];
                    for (j = 0; j < _n; j++)
                    {
                        if (i >= j)
                            h[i][j] = _qr[i][j];
                        else
                            h[i][j] = 0.0;
                    }
                }
                return h;
			}
		}

		/// <summary>Return the upper triangular factor, R.</summary>
		public double[][] R
		{
			get
			{
                double[][] r = new double[_m][];
                int i, j;
                for (i = 0; i < _n; i++)
                {
                    r[i] = new double[_n];
                    for (j = 0; j < _n; j++)
                    {
                        if (i < j)
                            r[i][j] = _qr[i][j];
                        else if (i == j)
                            r[i][j] = _diagR[i];
                        else
                            r[i][j] = 0.0;
                    }
                }
				return r;
			}
		}

		/// <summary>Generate and return the (economy-sized) orthogonal factor, Q.</summary>
		public double[][] Q
		{
			get
			{
                int i, j, k;
                double[][] q = new double[_m][];
                double s;

                for (i = 0; i < _m; i++)
                    q[i] = new double[_n];
                
				for (k = _n - 1; k >= 0; k--)
				{
					for (i = 0; i < _m; i++)
						q[i][k] = 0.0;
					q[k][k] = 1.0;

					for (j = k; j < _n; j++)
					{
						if (_qr[k][k] != 0)
						{
							s = 0.0;
							for (i = k; i < _m; i++)
								s += _qr[i][k] * q[i][j];
							s = (-s) / _qr[k][k];

							for (i = k; i < _m; i++)
								q[i][j] += s * _qr[i][k];
						}
					}
				}
				return q;
			}
		}

		#endregion
		
		#region Constructor
		
		/// <summary>QR Decomposition][computed by Householder reflections.</summary>
		/// <param name="matrix">a rectangular matrix</param>
        public QRDecomposition(double[][] matrix) : base(matrix)
		{
			// assign class variables
            _qr = new double[_m][];
            for (int i = 0; i < _m; i++)
                Array.Copy(matrix, _qr, _n);
			
			_diagR = new double[_n];

            InitializeQR();
		}

        private void InitializeQR()
        {
            double nrm;
            double s;
            int i, j, k;

            for (k = 0; k < _n; k++) // main loop.
            {
                // compute 2-norm of k-th column without under/overflow.
                nrm = 0;
                for (i = k; i < _m; i++)
                    nrm = Functions.GeometryFunctions.Hypot(nrm, _qr[i][k]);

                if (nrm != 0.0)
                {
                    // form k-th Householder vector.
                    if (_qr[k][k] < 0)
                        nrm = -nrm;

                    for (i = k; i < _m; i++)
                        _qr[i][k] /= nrm;
                    _qr[k][k] += 1.0;

                    // apply transformation to remaining columns.
                    for (j = k + 1; j < _n; j++)
                    {
                        s = 0.0;
                        for (i = k; i < _m; i++)
                            s += _qr[i][k] * _qr[i][j];
                        s = (-s) / _qr[k][k];
                        for (i = k; i < _m; i++)
                            _qr[i][j] += s * _qr[i][k];
                    }
                }
                _diagR[k] = -nrm;
            }
        }

		#endregion

		#region Public Methods
		
		/// <summary>Least squares solution of A * X = B</summary>
		/// <param name="b">a matrix with as many rows as A and any number of columns.</param>
		/// <returns>X that minimizes the two norm of Q * R * X - B.</returns>
		public double[][] Solve(double[][] b)
		{
            if (b.Length != _m)
				throw new ArgumentException("MatrixDouble row dimensions must agree.");

            if (!IsFullRank)
				throw new SystemException("Matrix is rank deficient.");

            int i, j, k;
			int nx = b[0].Length;
            double[][] x = new double[_m][];
            for (i = 0; i < _m; i++)
                Array.Copy(b, x, _n);
            double s;

			// compute Y = transpose(Q) * B
            for (k = 0; k < _n; k++)
				for (j = 0; j < nx; j++)
				{
					s = 0.0;
                    for (i = k; i < _m; i++)
						s += _qr[i][k] * x[i][j];
					s = -s / _qr[k][k];

                    for (i = k; i < _m; i++)
						x[i][j] += s * _qr[i][k];
				}

			// solve R * X = Y;
            for (k = _n - 1; k >= 0; k--)
			{
				for (j = 0; j < nx; j++)
					x[k][j] /= _diagR[k];

				for (i = 0; i < k; i++)
					for (j = 0; j < nx; j++)
						x[i][j] -= x[k][j] * _qr[i][k];
			}

            return MatrixDouble.SubMatrix(x, 0, _n - 1, 0, nx - 1);
		}

		#endregion
	}
}